

#include <stdio.h>
#include <string>
#include <cstdlib>
#include <sstream>
#include <list>
#include <termios.h>
#include <fcntl.h>
#include "Schwartz.h"
#include "../Yogurt/utility.h"
using namespace std;

Schwartz::Schwartz() {
	configureSerialPort();

}

void Schwartz::configureSerialPort() {
	struct termios options;
	xBee = open("/dev/ttySAC3", O_RDWR | O_NOCTTY | O_NDELAY);
	tcgetattr(xBee, &options);
	cfsetispeed(&options, B115200);
	cfsetospeed(&options, B115200);
	options.c_cflag = (options.c_cflag & ~CSIZE) | CS8;
	options.c_iflag = IGNBRK;
	options.c_lflag = 0;
	options.c_oflag = 0;
	options.c_cflag |= CLOCAL | CREAD;
	options.c_cc[VMIN] = 1;
	options.c_cc[VTIME] = 5;
	options.c_iflag &= ~(IXON|IXOFF|IXANY);
	options.c_cflag &= ~(PARENB | PARODD);
	tcsetattr(xBee,TCSANOW, &options);

}

void Schwartz::writeBytes(char msg) {
	write(xBee, (char*)msg, sizeof(msg));
}

char* Schwartz::readBytes() {
	char* msgSize[2];
	read(xBee, msgSize, 2);
	int size = atoi(*msgSize);
	char* incomming[size];
	incomming[0] = 0;
	int numBytes = read(xBee, incomming, size);
	if (numBytes < size) {
		wait(500);
		char* moreInc[size - numBytes];
		read(xBee, moreInc, size-numBytes);
		for(int k = numBytes; k==size; ++k) {
			incomming[k] = moreInc[size - k];
		}
	}
	return *incomming;
}


Message Schwartz::retrieveIncomming() {
	char* incomming = readBytes();
	if(incomming[0] == 0) {
		Message msg;
		msg.type_ = 0;
		return msg;
	}
	int type = atoi((char*)incomming[0]);
	int seekerID = atoi((char*)incomming[1]);
	switch (type) {
		case 1: {
			SeekerReport msg;
			msg.type_ = type;
			msg.seekerID_ = seekerID;
			int status = atoi((char*)incomming[2]);
			msg.pathStatus_ = status;
			int curLeg = atoi((char*)incomming[3]);
			msg.currentLeg_ = curLeg;
			int lastLeg = atoi((char*)incomming[4]);
			msg.lastRouteLegComplete_ = lastLeg;
			return msg;
		}
		case 2: {
			SeekerRoute msg;
			msg.type_ = type;
			msg.seekerID_ = seekerID;
			list<DIRECTION> route;
			int n = sizeof(incomming);
			for(int k = 2; k < n; ++k) {
				route.push_back(getDirection(atoi((char*)incomming[k])));
			}
			msg.rtDirections_ = route;
			return msg;
		}
	}
	Message msg;
	msg.type_ = 0;
	return msg;
}


void Schwartz::sendPath(list<pair<string, DIRECTION> > path, int seekerID) {
	int msgSize = path.size() + 4;
	char outgoing[msgSize];
	--msgSize;
	--msgSize;
	if(msgSize > 9) {
		std::string s;
		std::stringstream out;
		out << msgSize;
		s = out.str();
		outgoing[0] = s[0];
		outgoing[1] = s[1];
	} else {
		outgoing[0] = 0;
		outgoing[1] = msgSize;
	}
	outgoing[2] = 2;
	outgoing[3] = seekerID;
	int msgIdx = 4;
	list<pair<string, DIRECTION> >::iterator i;
	for (i = path.begin(); i != path.end(); i++) {
		outgoing[msgIdx] = i->second;
		++msgIdx;
	}
	writeBytes(*outgoing);
}

void Schwartz::sendReport(SeekerReport report) {
	char outgoing[7];
	outgoing[0] = 0;
	outgoing[1] = 5;
	outgoing[2] = report.type_;
	outgoing[3] = report.seekerID_;
	outgoing[4] = report.pathStatus_;
	outgoing[5] = report.currentLeg_;
	outgoing[6] = report.lastRouteLegComplete_;
	writeBytes(*outgoing);
}


Schwartz::~Schwartz() {

}
